/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-08-02 22:49:49
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-22 21:47:40
 * @FilePath: /esp32_motor/motor/algorithm /fluxObserver.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "fluxObserver.h"
#include <math.h>
#include "motor_priv.h"

const _iq27     _0_0_5       = _IQ27(0.05);
const _iq27     _0_5         = _IQ27(0.5);
const _iq27     _2_5         = _IQ27(2.5);
const _iq27     _0_3         = _IQ27(0.3);
const _iq27     _1_1         = _IQ27(1.1);
const _iq21     _0_0_1       = _IQ21(0.01);
const _iq21     _0_99        = _IQ21(0.99);

uint8_t         speedCalcCnt = 0;
_iq24           speedAcc     = 0;
_iq21           speedNow     = 0;

FluxObserverObj fluxOb;

/*初始化*/
esp_err_t FLUX_observerInit(void)
{
    fluxOb.rs         = _IQ27(MOTOR_RS);
    fluxOb.ls         = _IQ27(MOTOR_LS);
    fluxOb.flux       = _IQ27(MOTOR_FLUX);
    fluxOb.lambdaEst  = _IQ27(MOTOR_FLUX);
    fluxOb.lambdaMax  = _IQ27(MOTOR_FLUX * 2.5);
    fluxOb.lambdaMin  = _IQ27(MOTOR_FLUX * 0.3);
    fluxOb.flux2      = _IQ27(MOTOR_FLUX * MOTOR_FLUX);

    fluxOb.pllKp      = _IQ(20);
    fluxOb.pllKi      = _IQ(10);
    fluxOb.pllErr     = _IQ27(0.0F);
    fluxOb.pllInterg  = _IQ27(0.0F);
    fluxOb.pllUi      = _IQ27(0.0F);

    fluxOb.ctrlTs     = _IQ27(MOTOR_CYCLE / 1e6);
    fluxOb.speedGain  = _IQ21(1.F / ((MOTOR_CYCLE / 1e6) * 10.F * 7.0F));
    fluxOb.gain       = _IQ2(35e6);
    fluxOb.gainOdrive = _IQ2(1000 * 0.5 / MOTOR_FLUX);
    fluxOb.err        = _IQ27(0.0F);
    fluxOb.angle      = _IQ27(0);
    fluxOb.speedHz    = _IQ27(0.0f);

    fluxOb.x1         = _IQ27(0.0f);
    fluxOb.x2         = _IQ27(0.0f);
    fluxOb.x1Dot      = _IQ27(0.0f);
    fluxOb.x2Dot      = _IQ27(0.0f);

    speedCalcCnt      = 0;
    speedAcc          = _IQ27(0.0f);
    speedNow          = _IQ27(0.0f);

    return ESP_OK;
}

// void FLUX_observerOrtegaLambdaComp(FluxObserverPara para)
// {
//     const double lIa       = fluxOb.ls * para.iAlpha;
//     const double lIb       = fluxOb.ls * para.iBeta;
//     const double rIa       = fluxOb.rs * para.iAlpha;
//     const double rIb       = fluxOb.rs * para.iBeta;
//     const double gammaHalf = fluxOb.gain * 0.5;// 观测器增益的一半，公式8
//     SinCos       sinCos;

//     /*计算*/
//     double err = pow(fluxOb.lambdaEst, 2) - (pow(fluxOb.x1 - lIa, 2) + pow(fluxOb.x2 - lIb, 2));

//     // FLux linkage observer. See:
//     // https://cas.mines-paristech.fr/~praly/Telechargement/Conferences/2017_IFAC_Bernard-Praly.pdf
//     fluxOb.lambdaEst += 0.2 * gammaHalf * fluxOb.lambdaEst * -err * fluxOb.ctrlTs;

//     // Clamp the observed flux linkage (not sure if this is needed)
//     TruncateNumber(&(fluxOb.lambdaEst), fluxOb.flux * 0.3, fluxOb.flux * 2.5);

//     if (err > 0.0)
//     {
//         err = 0.0;
//     }

//     double x1Dot      = para.vAlpha - rIa + gammaHalf * (fluxOb.x1 - lIa) * err;
//     double x2Dot      = para.vBeta - rIb + gammaHalf * (fluxOb.x2 - lIb) * err;

//     fluxOb.x1         += x1Dot * fluxOb.ctrlTs;
//     fluxOb.x2         += x2Dot * fluxOb.ctrlTs;

//     fluxOb.iAlphaPrev = para.iAlpha;
//     fluxOb.iBetaPrev  = para.iBeta;

//     UTILS_NAN_ZERO(fluxOb.x1);
//     UTILS_NAN_ZERO(fluxOb.x2);

//     // Prevent the magnitude from getting too low, as that makes the angle very unstable.
//     double mag = NORM2_F(fluxOb.x1, fluxOb.x2);
//     if (mag < (fluxOb.flux * 0.5))
//     {
//         fluxOb.x1 *= 1.1;
//         fluxOb.x2 *= 1.1;
//     }

//     FastSinCos(fluxOb.qPllAngle, &sinCos);

//     fluxOb.pllErr    = (fluxOb.x2 - lIb) * sinCos.cosValue - (fluxOb.x1 - lIa) * sinCos.sinValue;
//     fluxOb.pllInterg += fluxOb.pllErr * fluxOb.pllKi;
//     fluxOb.pllUi     = fluxOb.pllErr * fluxOb.pllKp + fluxOb.pllInterg;

//     fluxOb.qPllAngle = fluxOb.qPllAngle + (int16_t)(fluxOb.pllUi * _Q2PI / _2PI);
//     fluxOb.qPllAngle = ElectricalQAngle(fluxOb.qPllAngle);

//     if (speedCalcCnt < 10)
//     {
//         speedAcc += fluxOb.pllUi;
//         speedCalcCnt++;
//     }
//     else
//     {
//         speedNow       = speedAcc / (fluxOb.ctrlTs * 10) / 7;
//         fluxOb.speedHz = fluxOb.speedHz * 0.99f + speedNow * 0.01f;

//         speedAcc       = 0.0f;
//         speedCalcCnt   = 0;
//     }
// }

// void FLUX_observerMxlemming(FluxObserverPara para)
// {
//     const double rIa = fluxOb.rs * para.iAlpha;
//     const double rIb = fluxOb.rs * para.iBeta;
//     SinCos       sinCos;

//     fluxOb.x1 += (para.vAlpha - rIa) * fluxOb.ctrlTs - fluxOb.ls * (para.iAlpha - fluxOb.iAlphaPrev);
//     fluxOb.x2 += (para.vBeta - rIb) * fluxOb.ctrlTs - fluxOb.ls * (para.iBeta - fluxOb.iBetaPrev);

//     TruncateNumberAbs(&(fluxOb.x1), fluxOb.flux);
//     TruncateNumberAbs(&(fluxOb.x2), fluxOb.flux);

//     fluxOb.iAlphaPrev = para.iAlpha;
//     fluxOb.iBetaPrev  = para.iBeta;

//     UTILS_NAN_ZERO(fluxOb.x1);
//     UTILS_NAN_ZERO(fluxOb.x2);

//     // Prevent the magnitude from getting too low, as that makes the angle very unstable.
//     double mag = NORM2_F(fluxOb.x1, fluxOb.x2);
//     if (mag < (fluxOb.flux * 0.5))
//     {
//         fluxOb.x1 *= 1.1;
//         fluxOb.x2 *= 1.1;
//     }

//     FastSinCos(fluxOb.qPllAngle, &sinCos);

//     fluxOb.pllErr    = fluxOb.x2 * sinCos.cosValue - fluxOb.x1 * sinCos.sinValue;
//     fluxOb.pllInterg += fluxOb.pllErr * fluxOb.pllKi;
//     fluxOb.pllUi     = fluxOb.pllErr * fluxOb.pllKp + fluxOb.pllInterg;

//     fluxOb.qPllAngle = fluxOb.qPllAngle + (int16_t)(fluxOb.pllUi * _Q2PI / _2PI);
//     fluxOb.qPllAngle = ElectricalQAngle(fluxOb.qPllAngle);

//     if (speedCalcCnt < 10)
//     {
//         speedAcc += fluxOb.pllUi;
//         speedCalcCnt++;
//     }
//     else
//     {
//         speedNow       = _IQmpy(_IQdiv(speedAcc, _IQmpyI32(fluxOb.ctrlTs, 10)), _1_7);
//         fluxOb.speedHz = _IQmpy(fluxOb.speedHz, _0_99) + _IQmpy(speedNow, _0_0_1);

//         speedAcc       = 0;
//         speedCalcCnt   = 0;
//     }
// }

void FLUX_observerMxlemmingLambdaComp(FluxObserverPara para)
{
    const _iq27 vAlpha = _IQtoIQ27(para.vAlpha);
    const _iq27 vBeta  = _IQtoIQ27(para.vBeta);
    const _iq27 iAlpha = _IQtoIQ27(para.iAlpha);
    const _iq27 iBeta  = _IQtoIQ27(para.iBeta);
    const _iq27 rIa    = _IQ27mpy(fluxOb.rs, iAlpha);
    const _iq27 rIb    = _IQ27mpy(fluxOb.rs, iBeta);
    SinCos      sinCos;

    fluxOb.x1 += (_IQ27mpy(vAlpha - rIa, fluxOb.ctrlTs) - _IQ27mpy(fluxOb.ls, iAlpha - fluxOb.iAlphaPrev));
    fluxOb.x2 += (_IQ27mpy(vBeta - rIb, fluxOb.ctrlTs) - _IQ27mpy(fluxOb.ls, iBeta - fluxOb.iBetaPrev));

    _iq27 err =
        _IQ27mpy(fluxOb.lambdaEst, fluxOb.lambdaEst) - _IQ27mpy(fluxOb.x1, fluxOb.x1) - _IQ27mpy(fluxOb.x2, fluxOb.x2);
    fluxOb.err       = _IQmpyIQX(-err, 27, fluxOb.gain, 2);
    fluxOb.lambdaEst += _IQ27mpy(_0_0_5, _IQ27mpy(_IQ27mpyIQX(fluxOb.err, 24, fluxOb.lambdaEst, 27), fluxOb.ctrlTs));

    // Clamp the observed flux linkage (not sure if this is needed)
    fluxOb.lambdaEst = _IQsat(fluxOb.lambdaEst, fluxOb.lambdaMax, fluxOb.lambdaMin);
    TruncateNumberAbs(&(fluxOb.x1), fluxOb.lambdaEst);
    TruncateNumberAbs(&(fluxOb.x2), fluxOb.lambdaEst);

    fluxOb.iAlphaPrev = iAlpha;
    fluxOb.iBetaPrev  = iBeta;

    UTILS_NAN_ZERO(fluxOb.x1);
    UTILS_NAN_ZERO(fluxOb.x2);

    // Prevent the magnitude from getting too low, as that makes the angle very unstable.
    _iq27 mag = _IQ27mag(fluxOb.x1, fluxOb.x2);
    if (mag < _IQ27mpy(fluxOb.flux, _0_5))
    {
        fluxOb.x1 = _IQ27mpy(fluxOb.x1, _1_1);
        fluxOb.x2 = _IQ27mpy(fluxOb.x2, _1_1);
    }

    FastSinCos(fluxOb.angle, &sinCos);

    fluxOb.pllErr = _IQ27mpy(fluxOb.x2, _IQtoIQ27(sinCos.cosValue)) - _IQ27mpy(fluxOb.x1, _IQtoIQ27(sinCos.sinValue));
    fluxOb.pllInterg += _IQ27mpyIQX(fluxOb.pllErr, 27, fluxOb.pllKi, 24);
    fluxOb.pllUi     = _IQ27mpyIQX(fluxOb.pllErr, 27, fluxOb.pllKp, 24) + fluxOb.pllInterg;

    _iq24 pllUi      = _IQ27toIQ(fluxOb.pllUi);
    fluxOb.angle     += pllUi;
    fluxOb.angle     = ElectricalIQAngle(fluxOb.angle);

    if (speedCalcCnt < 10)
    {
        speedAcc += pllUi;
        speedCalcCnt++;
    }
    else
    {
        speedAcc       = _IQtoIQ21(speedAcc);
        speedNow       = _IQ21mpy(speedAcc, fluxOb.speedGain);
        fluxOb.speedHz = _IQ21mpy(fluxOb.speedHz, _0_99) + _IQ21mpy(speedNow, _0_0_1);

        speedAcc       = 0;
        speedCalcCnt   = 0;
    }
}

// void FLUX_observerOriginal(FluxObserverPara para)
// {
//     const double lIa       = fluxOb.ls * para.iAlpha;
//     const double lIb       = fluxOb.ls * para.iBeta;
//     const double rIa       = fluxOb.rs * para.iAlpha;
//     const double rIb       = fluxOb.rs * para.iBeta;
//     const double lambda2   = fluxOb.flux * fluxOb.flux;// 磁链的平方
//     const double gammaHalf = fluxOb.gain * 0.5;        // 观测器增益的一半，公式8
//     SinCos       sinCos;

//     // Original
//     double err = lambda2 - (pow(fluxOb.x1 - lIa, 2) + pow(fluxOb.x2 - lIb, 2));
//     if (err > 0.0)
//     {
//         err = 0.0;
//     }
//     double x1Dot = -rIa + para.vAlpha + gammaHalf * (fluxOb.x1 - lIa) * err;
//     double x2Dot = -rIb + para.vBeta + gammaHalf * (fluxOb.x2 - lIb) * err;
//     fluxOb.x1    += x1Dot * fluxOb.ctrlTs;
//     fluxOb.x2    += x2Dot * fluxOb.ctrlTs;

//     UTILS_NAN_ZERO(fluxOb.x1);
//     UTILS_NAN_ZERO(fluxOb.x2);

//     // Prevent the magnitude from getting too low, as that makes the angle very unstable.
//     double mag = NORM2_F(fluxOb.x1, fluxOb.x2);
//     if (mag < (fluxOb.flux * 0.5))
//     {
//         fluxOb.x1 *= 1.1;
//         fluxOb.x2 *= 1.1;
//     }

//     FastSinCos(fluxOb.qPllAngle, &sinCos);

//     fluxOb.pllErr    = (fluxOb.x2 - lIb) * sinCos.cosValue - (fluxOb.x1 - lIa) * sinCos.sinValue;
//     fluxOb.pllInterg += fluxOb.pllErr * fluxOb.pllKi;
//     fluxOb.pllUi     = fluxOb.pllErr * fluxOb.pllKp + fluxOb.pllInterg;

//     fluxOb.qPllAngle = fluxOb.qPllAngle + (int16_t)(fluxOb.pllUi * _Q2PI / _2PI);
//     fluxOb.qPllAngle = ElectricalQAngle(fluxOb.qPllAngle);

//     if (speedCalcCnt < 10)
//     {
//         speedAcc += fluxOb.pllUi;
//         speedCalcCnt++;
//     }
//     else
//     {
//         speedNow       = speedAcc / (fluxOb.ctrlTs * 10) / 7;
//         fluxOb.speedHz = fluxOb.speedHz * 0.99f + speedNow * 0.01f;

//         speedAcc       = 0.0f;
//         speedCalcCnt   = 0;
//     }
// }

void FLUX_observer(FluxObserverPara para)
{
    const _iq27 vAlpha = _IQtoIQ27(para.vAlpha);
    const _iq27 vBeta  = _IQtoIQ27(para.vBeta);
    const _iq27 iAlpha = _IQtoIQ27(para.iAlpha);
    const _iq27 iBeta  = _IQtoIQ27(para.iBeta);
    const _iq27 rIa    = _IQ27mpy(fluxOb.rs, iAlpha);
    const _iq27 rIb    = _IQ27mpy(fluxOb.rs, iBeta);
    const _iq27 lIa    = _IQ27mpy(fluxOb.ls, iAlpha);
    const _iq27 lIb    = _IQ27mpy(fluxOb.ls, iBeta);
    SinCos      sinCos;

    //calc alpha asix flux
    _iq27 vAlphaErr = vAlpha - rIa;
    vAlphaErr       += _IQ27mpyIQX(fluxOb.x1, 27, fluxOb.err, 24);
    fluxOb.x1Dot    += _IQ27mpy(vAlphaErr, fluxOb.ctrlTs);

    //calc beta asix flux
    _iq27 vBetaErr = vBeta - rIb;
    vBetaErr       += _IQ27mpyIQX(fluxOb.x2, 27, fluxOb.err, 24);
    fluxOb.x2Dot   += _IQ27mpy(vBetaErr, fluxOb.ctrlTs);

    //calc flux in rotor
    fluxOb.x1 = fluxOb.x1Dot - lIa;
    fluxOb.x2 = fluxOb.x2Dot - lIb;

    /*计算误差*/
    fluxOb.err = fluxOb.flux2 - _IQ27mpy(fluxOb.x1, fluxOb.x1) - _IQ27mpy(fluxOb.x2, fluxOb.x2);
    fluxOb.err = _IQmpyIQX(fluxOb.err, 27, fluxOb.gain, 2);

    FastSinCos(fluxOb.angle, &sinCos);

    fluxOb.pllErr = _IQ27mpy(fluxOb.x2, _IQtoIQ27(sinCos.cosValue)) - _IQ27mpy(fluxOb.x1, _IQtoIQ27(sinCos.sinValue));
    fluxOb.pllInterg += _IQ27mpyIQX(fluxOb.pllErr, 27, fluxOb.pllKi, 24);
    fluxOb.pllUi     = _IQ27mpyIQX(fluxOb.pllErr, 27, fluxOb.pllKp, 24) + fluxOb.pllInterg;

    _iq24 pllUi      = _IQ27toIQ(fluxOb.pllUi);
    fluxOb.angle     += pllUi;
    fluxOb.angle     = ElectricalIQAngle(fluxOb.angle);

    if (speedCalcCnt < 10)
    {
        speedAcc += pllUi;
        speedCalcCnt++;
    }
    else
    {
        speedAcc       = _IQtoIQ21(speedAcc);
        speedNow       = _IQ21mpy(speedAcc, fluxOb.speedGain);
        fluxOb.speedHz = _IQ21mpy(fluxOb.speedHz, _0_99) + _IQ21mpy(speedNow, _0_0_1);

        speedAcc       = 0;
        speedCalcCnt   = 0;
    }
}

// void FLUX_observerOdrive(FluxObserverPara para)
// {
//     const double lIa = fluxOb.ls * para.iAlpha;
//     const double lIb = fluxOb.ls * para.iBeta;
//     const double rIa = fluxOb.rs * para.iAlpha;
//     const double rIb = fluxOb.rs * para.iBeta;
//     SinCos       sinCos;
//     double       eta1, eta2;

//     fluxOb.x1        += (para.vAlpha - rIa) * fluxOb.ctrlTs;
//     eta1             = fluxOb.x1 - lIa;
//     fluxOb.x2        += (para.vBeta - rIb) * fluxOb.ctrlTs;
//     eta2             = fluxOb.x2 - lIb;

//     double etaFactor = fluxOb.flux2 * (fluxOb.gainOdrive - (pow(eta1, 2) + pow(eta2, 2)));
//     fluxOb.x1        += etaFactor * eta1 * fluxOb.ctrlTs;
//     eta1             = fluxOb.x1 - lIa;
//     fluxOb.x2        += etaFactor * eta2 * fluxOb.ctrlTs;
//     eta2             = fluxOb.x2 - lIb;

//     FastSinCos(fluxOb.qPllAngle, &sinCos);

//     fluxOb.pllErr    = eta2 * sinCos.cosValue - eta1 * sinCos.sinValue;
//     fluxOb.pllInterg += fluxOb.pllErr * fluxOb.pllKi;
//     fluxOb.pllUi     = fluxOb.pllErr * fluxOb.pllKp + fluxOb.pllInterg;

//     fluxOb.qPllAngle = fluxOb.qPllAngle + (int16_t)(fluxOb.pllUi * _Q2PI / _2PI);
//     fluxOb.qPllAngle = ElectricalQAngle(fluxOb.qPllAngle);

//     if (speedCalcCnt < 10)
//     {
//         speedAcc += fluxOb.pllUi;
//         speedCalcCnt++;
//     }
//     else
//     {
//         speedNow       = speedAcc / (fluxOb.ctrlTs * 10) / 7;
//         fluxOb.speedHz = fluxOb.speedHz * 0.99f + speedNow * 0.01f;

//         speedAcc       = 0.0f;
//         speedCalcCnt   = 0;
//     }
// }


//((vAlpha-rIa)*t-rIa)*0.5*gain*err